﻿namespace Auxiliary.VecMath
{
    public sealed class Matrix4D
    {
        private const int Size = 4;

        private readonly float[,] _values = new float[Size, Size];
        
        public Matrix4D(float value)
        {
            for (int i = 0; i < Size; i++)
            {
                _values[i, i] = value;
            }
        }

        public Matrix4D(float[] diagonal)
        {
            for (int i = 0; i < Size; i++)
            {
                _values[i, i] = diagonal[i];
            }
        }

        public Matrix4D(float[,] matrix)
        {
            for (int i = 0; i < Size; i++)
            {
                for (int j = 0; j < Size; j++)
                {
                    _values[i, j] = matrix[i, j];
                }
            }
        }

        public Matrix4D(Matrix4D matrix)
        {
            for (int i = 0; i < Size; i++)
            {
                for (int j = 0; j < Size; j++)
                {
                    _values[i, j] = matrix[i, j];
                }
            }
        }

        public static explicit operator Matrix4D(Matrix3D matrix)
        {
            var result = Zero;

            for (int i = 0; i < Size - 1; i++)
            {
                for (int j = 0; j < Size - 1; j++)
                {
                    result[i, j] = matrix[i, j];
                }

                result[i, Size - 1] = 0;
                result[Size - 1, i] = 0;
            }
            result[Size - 1, Size - 1] = 1;

            return result;
        }
        
        public static Matrix4D operator -(Matrix4D source)
        {
            var result = new float[Size, Size];

            for (int i = 0; i < Size; i++)
            {
                for (int j = 0; j < Size; j++)
                {
                    result[i, j] = -source[i, j];
                }
            }

            return new Matrix4D(result);
        }

        public static Matrix4D operator -(Matrix4D left, Matrix4D right)
        {
            var result = new float[Size, Size];

            for (int i = 0; i < Size; i++)
            {
                for (int j = 0; j < Size; j++)
                {
                    result[i, j] = left[i, j] - right[i, j];
                }
            }

            return new Matrix4D(result);
        }

        public static Matrix4D operator +(Matrix4D left, Matrix4D right)
        {
            var result = new float[Size, Size];

            for (int i = 0; i < Size; i++)
            {
                for (int j = 0; j < Size; j++)
                {
                    result[i, j] = left[i, j] + right[i, j];
                }
            }

            return new Matrix4D(result);
        }

        public static Matrix4D operator *(Matrix4D left, float right)
        {
            var result = new float[Size, Size];

            for (int i = 0; i < Size; i++)
            {
                for (int j = 0; j < Size; j++)
                {
                    result[i, j] = left[i, j] * right;
                }
            }

            return new Matrix4D(result);
        }

        public static Matrix4D operator *(float left, Matrix4D right)
        {
            return right * left;
        }

        public static Matrix4D operator *(Matrix4D left, Matrix4D right)
        {
            var result = new float[Size, Size];

            for (int i = 0; i < Size; i++)
            {
                for (int j = 0; j < Size; j++)
                {
                    result[i, j] = 0;

                    for (int k = 0; k < Size; k++)
                    {
                        result[i, j] += left[i, k] * right[k, j];
                    }
                }
            }

            return new Matrix4D(result);
        }

        public static Vector4D operator *(Matrix4D left, Vector4D right)
        {
            var result = new float[Size];

            for (int i = 0; i < Size; i++)
            {
                result[i] = 0;

                for (int k = 0; k < Size; k++)
                {
                    result[i] += left[i, k] * right[k];
                }
            }

            return new Vector4D(result);
        }

        public static Matrix4D operator /(Matrix4D left, float right)
        {
            var result = new float[Size, Size];

            for (int i = 0; i < Size; i++)
            {
                for (int j = 0; j < Size; j++)
                {
                    result[i, j] = left[i, j] / right;
                }
            }

            return new Matrix4D(result);
        }
        
        public float[] ToArray()
        {
            var result = new float[_values.Length];

            for (int i = 0; i < Size; i++)
            {
                for (int j = 0; j < Size; j++)
                {
                    result[i * Size + j] = _values[i, j];
                }
            }

            return result;
        }

        public Matrix4D Inverse()
        {
            var result = new Matrix4D(this);

            var temp = Zero;

            for (int k = 0; k < Size; k++)
            {
                for (int i = 0; i < Size; i++)
                {
                    for (int j = 0; j < Size; j++)
                    {
                        if (i == k)
                        {
                            if (j == k)
                            {
                                temp[i, j] = 1.0f / result[i, j];
                            }
                            else
                            {
                                temp[i, j] = -result[i, j] / result[k, k];
                            }
                        }
                        else
                        {
                            if (j == k)
                            {
                                temp[i, j] = result[i, k] / result[k, k];
                            }
                            else
                            {
                                temp[i, j] = result[i, j] - result[k, j] * result[i, k] / result[k, k];
                            }
                        }
                    }
                }

                for (int i = 0; i < Size; i++)
                {
                    for (int j = 0; j < Size; j++)
                    {
                        result[i, j] = temp[i, j];
                    }
                }
            }

            return result;
        }

        public Matrix4D Transpose()
        {
            var matrix = Zero;

            for (int i = 0; i < Size; i++)
            {
                for (int j = 0; j < Size; j++)
                {
                    matrix[i, j] = this[j, i];
                }
            }

            return matrix;
        }
        
        public float this[int row, int col]
        {
            get
            {
                return _values[row, col];
            }

            set
            {
                _values[row, col] = value;
            }
        }

        public static Matrix4D Zero
        {
            get
            {
                return new Matrix4D(0);
            }
        }

        public static Matrix4D Identity
        {
            get
            {
                return new Matrix4D(1);
            }
        }
    }
}
